/**
  * @file     	task.c
  * @author   	YangHaifeng
  * @email   	2995339193@qq.com
  * @version	V0.0
  * @license  	GNU General Public License (GPL)  
  * @detail		please follow the license!!!
  * @attention
  *  this file is aim to do something with task									
  *   																	
  * @htmlonly 
  * <span style="font-weight: bold">History</span> 
  * @endhtmlonly 
  * Version|Auther|Describe
  * ------|----|-------- 
  * V0.0|YangHaifeng|Create File
  * <h2><center>&copy;COPYRIGHT YangHaifeng All Rights Reserved.</center></h2>
  */ 
  
  
#include "task.h"



int32_t encoder_L=0;
int32_t encoder_R=0;


/**
 * @brief 主任务
 * @param None
 * @retval None
 * @author YangHaifeng
 */
void main_task(void)
{
	encoder_read(&encoder_L,&encoder_R);

	switch(option)
	{
		case 0:question_1();break;
		case 1:question_2();break;
		case 2:question_3();break;
		case 3:question_4();break;
	}
}


//内圈4.88
//外圈5.48	


/**
 * @brief 第1题
 * @param None
 * @retval None
 * @author Yanghaifeng
 */
void question_1(void)
{	
	const static float v30 = 14.4;//14.05
	static uint32_t clock = 0;
	static uint8_t flag = 0;
	int pwm,bios;
	
	switch(flag)
	{
		case 0:
		{
			if(clock<126)
			{
				pwm = pid_inc(v30,encoder_L+encoder_R);
				bios = track_30_pid(-50,openmv.rho);
				motor_set(pwm+bios,pwm-bios);
				clock++;
			}
			else 
			{
				clock = 0;
				flag = 1;
			}
		}break;
		case 1:
		{
			if(clock<1700)
			{
				pwm = pid_inc(v30,encoder_L+encoder_R);
				bios = track_30_pid(-40,openmv.rho);
				motor_set(pwm+bios,pwm-bios);
				clock++;
			}
			else 
			{
				clock = 0;
				flag = 2;
			}
		}break;
		case 2:
		{
			if(clock<100)
			{
				buzzer_on();
				pwm = pid_inc(0,encoder_L+encoder_R);
				motor_set(pwm,pwm);	
				clock++;			
			}
			else 
			{
				buzzer_off();
				clock = 0;
				flag = 3;
			}
		}break;
		case 3:
		{
			pwm = pid_inc(0,encoder_L+encoder_R);
			motor_set(pwm,pwm);
		}break;
	}

}




/**
 * @brief 第2题
 * @param None
 * @retval None
 * @author Yanghaifeng
 */
void question_2(void)
{
	const static float v50 = 23.3;
	static uint32_t clock = 0;
	static uint8_t flag = 0;
	int pwm,bios;

	
	switch(flag)
	{
		case 0:
		{
			if(clock<2300)
			{
				pwm = pid_inc(v50,encoder_L+encoder_R);
				bios = track_50_pid(-50,openmv.rho);
				motor_set(pwm+bios,pwm-bios);	
				clock++;			
			}
			else 
			{
				flag = 1;
				clock = 0;
			}
		}break;
		case 1:
		{
			if(clock<100)
			{
				buzzer_on();
				pwm = pid_inc(0,encoder_L+encoder_R);
				motor_set(pwm,pwm);
				clock++;
			}
			else 
			{
				buzzer_off();
				flag = 2;
				clock = 0;
			}
		}break;
		case 2:
		{
			pwm = pid_inc(0,encoder_L+encoder_R);
			motor_set(pwm,pwm);
		}break;
	}
}


/**
 * @brief 第3题
 * @param None
 * @retval None
 * @author Yanghaifeng
 */
void question_3(void)
{
	const static float v50 = 23.8;//23.3
	const static float v30 = 14.2;//14.05
	static uint32_t clock = 0;
	static uint8_t flag = 0;
	int pwm,bios;
		
	switch(flag)
	{
		//外
		case 0:
		{
			if(clock<1096)
			{
				pwm = pid_inc(v50,encoder_L+encoder_R);
				bios = track_50_pid(-50,openmv.rho);
				motor_set(pwm+bios,pwm-bios);
				clock++;
			}
			else 
			{
				clock = 0;
				flag = 1;
			}
		}break;
		//外
		case 1:
		{
			if(clock<1096)
			{
				//外圈减速
				if(clock>200 && clock<400)
				{
					pwm = pid_inc(v30,encoder_L+encoder_R);
					bios = track_30_pid(-40,openmv.rho);
					motor_set(pwm+bios,pwm-bios);
				}
				else 
				{
					pwm = pid_inc(v50,encoder_L+encoder_R);
					bios = track_50_pid(-50,openmv.rho);
					motor_set(pwm+bios,pwm-bios);
				}
				clock++;
			}
			else 
			{
				clock = 0;
				flag = 2;
			}
		}break;
		//内
		case 2:
		{
			if(clock<1050)//976
			{
				if(clock>50&&clock<350)
				{
					led_on();
				}
				else
				{ 
					led_off();
				}
				pwm = pid_inc(v50,encoder_L+encoder_R);
				bios = track_50_pid(20,openmv.rho);
				motor_set(pwm+bios,pwm-bios);
				clock++;
			}
			else 
			{
				clock = 0;
				flag = 3;
			}
		}break;
		case 3:
		{
			if(clock<100)
			{
				buzzer_on();
				pwm = pid_inc(0,encoder_L+encoder_R);
				motor_set(pwm,pwm);	
				clock++;
			}
			else 
			{
				buzzer_off();
				clock = 0;
				flag = 4;
			}
		}break;
		case 4:
		{
			pwm = pid_inc(0,encoder_L+encoder_R);
			motor_set(pwm,pwm);
		}break;
	}
}




/**
 * @brief 第4题
 * @param None
 * @retval None
 * @author Yanghaifeng
 */
void question_4(void)
{
}



